﻿using System;
using System.Collections.Generic;
using System.Text;

namespace point2bmp
{
    public class AStarPath
    {
        bool[] CellMap;    // 可行走的点
        int CellWidth;    // Cell宽度，Cell横向有多少个点。

        class AStarNode
        {
            public MyPoint point;
            public int g;
            public int h;
            public AStarNode parent;
            public List<MyPoint> neighbour;
            public bool closed;
        }

        int CellHeight;
        AStarNode[] PathNodeMap;
        QuadTree tree;  // 用啦检索四叉树用的。

        public void Init(bool[] cellmap, int cellwidth)
        {
            CellMap = new bool[cellmap.Length];
            CellWidth = cellwidth;
            for (int i = 0; i < cellmap.Length; i++)
                CellMap[i] = cellmap[i];

            CellHeight = CellMap.Length / CellWidth;
            if (CellHeight * CellWidth != CellMap.Length)
            {
                Console.WriteLine("尺寸错误。");
                return;
            }

            tree = new QuadTree();
            for (int i = 0; i < CellMap.Length; i++)
            {
                if (CellMap[i])
                {
                    tree.Fill(new MyPoint() { x = i % CellWidth, y = i / CellWidth }, 0, 0, CellWidth, CellHeight);
                }
            }
        }
        MyPoint GetNearValidWayPoint(MyPoint p)
        {
            if (CellMap[GetCellIdx(p)])
                return p;

            var ret = tree.Search(p, 0, 0, CellWidth, CellHeight);
            if (ret == null)
                return p;

            return ret;
        }
        public List<MyPoint> GetPath(MyPoint start, MyPoint end)
        {
            // 清理缓存。
            PathNodeMap = null;

            ValidateMyPoint(ref start);
            ValidateMyPoint(ref end);


            var startNode = CreatePathNode(GetNearValidWayPoint(start));
            var endNode = CreatePathNode(GetNearValidWayPoint(end));

            List<AStarNode> openlist = new List<AStarNode>();

            openlist.Add(startNode);
            bool bfind = false;
            while (openlist.Count > 0)
            {
                var current = openlist[0];
                openlist.RemoveAt(0);
                current.closed = true;

                if (current == endNode)
                {
                    bfind = true;
                    break;
                }

                foreach (var nbor in current.neighbour)
                {
                    var nborNode = CreatePathNode(nbor);
                    if (nborNode.closed)
                        continue;

                    var costToNeighbour = current.g + GetDist(nborNode.point, current.point);
                    var b = openlist.Contains(nborNode);
                    if (costToNeighbour < nborNode.g || !b)
                    {
                        nborNode.g = costToNeighbour;
                        nborNode.h = GetDist(nborNode.point, endNode.point);
                        nborNode.parent = current;

                        if (!b)
                        {
                            // 添加到开表中，cost值小的排在前面
                            var cost = nborNode.g + nborNode.h;
                            int idx = openlist.Count;
                            for (int i = 0; i < openlist.Count; i++)
                            {
                                if (cost < openlist[i].g + openlist[i].h)
                                {
                                    idx = i;
                                    break;
                                }
                            }
                            openlist.Insert(idx, nborNode);
                        }
                    }
                }
            }

            if (bfind)
            {
                // 打印
                var ret = TracePath(startNode, endNode);
                if (ret.Count > 0)
                {
                    ret.Add(start);
                    ret.Insert(0, end);
                    return ret;
                }
            }


            return null;
        }

        void ValidateMyPoint(ref MyPoint point)
        {
            if (point.x < 0)
                point.x = 0;
            if (point.x > CellWidth - 1)
                point.x = CellWidth - 1;
            if (point.y < 0)
                point.y = 0;
            if (point.y > CellHeight - 1)
                point.y = CellHeight - 1;
        }

        MyPoint GetNeighbour(MyPoint point, int x, int y)
        {
            // if neighbour can arrive.
            if (point.x + x < 0)
                return null;
            if (point.y + y < 0)
                return null;
            if (point.x + x >= CellWidth - 1)
                return null;
            if (point.y + y >= CellHeight - 1)
                return null;
            if (CellMap[(point.y + y) * CellWidth + (point.x + x)])
                return new MyPoint() { x = point.x + x, y = point.y + y };
            return null;
        }

        AStarNode CreatePathNode(MyPoint point)
        {
            if (PathNodeMap == null)
            {
                PathNodeMap = new AStarNode[CellMap.Length];
            }

            var idx = GetCellIdx(point);
            if (PathNodeMap[idx] != null)
                return PathNodeMap[idx];

            var ret = new AStarNode();
            PathNodeMap[idx] = ret;
            ret.point = point;
            // top,right,bottom,left.
            if (ret.neighbour == null)
                ret.neighbour = new List<MyPoint>();
            {
                var nbor = GetNeighbour(point, 0, 1);
                if (nbor != null)
                    ret.neighbour.Add(nbor);
            }
            {
                var nbor = GetNeighbour(point, 1, 0);
                if (nbor != null)
                    ret.neighbour.Add(nbor);
            }
            {
                var nbor = GetNeighbour(point, 0, -1);
                if (nbor != null)
                    ret.neighbour.Add(nbor);
            }
            {
                var nbor = GetNeighbour(point, -1, 0);
                if (nbor != null)
                    ret.neighbour.Add(nbor);
            }
            return ret;
        }
        int GetCellIdx(MyPoint point)
        {
            return point.x + point.y * CellWidth;
        }
        int GetDist(MyPoint p1, MyPoint p2)
        {
            return Math.Abs(p2.x - p1.x) + Math.Abs(p2.y - p1.y);
        }

        // 从end遍历到start
        static List<MyPoint> TracePath(AStarNode start, AStarNode end)
        {
            var ret = new List<MyPoint>();
            var current = end;
            while (current != start)
            {
                ret.Add(current.point);
                current = current.parent;
            }
            if (ret.Count > 0)
            {
                ret.Add(start.point);
            }
            return ret;
        }
    }
}
